//
// Created by sunyl on 25-5-15.
//
#include "main.h"
#include "Motor.h"
#include "GPIO.h"
#include "stm32f1xx_hal.h"
#include "retarget.h"
#include "tim.h"
#define SPEED_BUFFER 7

int direction = 0 ;

uint16_t Motor_Pulse = 0 ;

//A->AB->B->BC->C->CD->D->DA
void MOTOR_CONTROLD(uint8_t step, uint8_t direction)
{
    if (step>255) step=0;
    uint8_t __step = step%8 ;

    if( 2 == direction )
        __step = 7 - __step ;

    switch(__step)
    {
        //A
        case 0:
            MOTOR_1_ON;
            MOTOR_2_OFF;
            MOTOR_3_OFF;
            MOTOR_4_OFF;
            break ;

            //AB
        case 1:
            MOTOR_1_ON;
            MOTOR_2_ON;
            MOTOR_3_OFF;
            MOTOR_4_OFF;
            break ;

            //B
        case 2:
            MOTOR_1_OFF;
            MOTOR_2_ON;
            MOTOR_3_OFF;
            MOTOR_4_OFF;
            break ;

            //BC
        case 3:
            MOTOR_1_OFF;
            MOTOR_2_ON;
            MOTOR_3_ON;
            MOTOR_4_OFF;
            break ;

            //C
        case 4:
            MOTOR_1_OFF;
            MOTOR_2_OFF;
            MOTOR_3_ON;
            MOTOR_4_OFF;
            break ;

            //CD
        case 5:
            MOTOR_1_OFF;
            MOTOR_2_OFF;
            MOTOR_3_ON;
            MOTOR_4_ON;
            break ;

            //D
        case 6:
            MOTOR_1_OFF;
            MOTOR_2_OFF;
            MOTOR_3_OFF;
            MOTOR_4_ON;
            break ;
            //DA
        case 7:
            MOTOR_1_ON;
            MOTOR_2_OFF;
            MOTOR_3_OFF;
            MOTOR_4_ON;
            break ;
    }
}

void TEST_MOTOR3(void)
{
    uint8_t sig;
    if( motor_flag==1 )
    {
        printf("Direction:Forward\r\n");
        while (motor_flag!=0xff)
        {
            for (int i=0; i<8; i++)
            {
                MOTOR_CONTROLD(i,forward);
                delay_us(motor_speed);
                motor_juli_zheng++;
            }
            if(motor_speed > 1100)
            {
                motor_speed=motor_speed-100;
            }
        }
    }
    else if( motor_flag==2 )
    {
        printf("Direction:backward\r\n");
        while (motor_flag!=0xff)
        {

            for (int i=0; i<8; i++)
            {
                MOTOR_CONTROLD(i,backward);
                delay_us(motor_speed);
                motor_juli_fan++;
            }

            if(motor_speed > 1100)
            {
                motor_speed=motor_speed-100;
            }
        }
    }
    else if( motor_flag== 0xff )
    {
        motor_flag=0;
        if(motor_juli_zheng > motor_juli_fan)
        {
            juli=((motor_juli_zheng-motor_juli_fan)*0.9/360)*4;
            sig = 1;
        }
        else if(motor_juli_zheng < motor_juli_fan)
        {
            juli=((motor_juli_fan-motor_juli_zheng)*0.9/360)*4;
            sig = 2;
        }
        else
        {
            juli=0;
        }
        uint32_t delta_time_ms = __HAL_TIM_GET_COUNTER(&htim7);  // 单位：微秒
        printf("stop!\r\n");
        printf("lead:");
        if(sig == 1){printf("+");}
        else{printf("-");}
        printf("%.2fmm\r\n",juli);
        motor_juli_zheng=0;
        motor_juli_fan=0;
    }
}

void TEST_MOTOR2(void)
{
    uint8_t step = 0;
    uint32_t motor_speed = 1000;  // 初始速度
    float speed_factor = 0.7;     // 指数加速因子，表示每次递减 90%

    while (1)  // 无限循环正转
    {
        MOTOR_CONTROLD(step, backward);
        step = (step + 1) % 8;  // 下一步

        delay_us(motor_speed);         // 控制转速（延时可调）

        if (motor_speed >728)  // 当速度大于500时，加速
        {
            motor_speed = motor_speed - 5;  // 应用指数加速
            //motor_speed = motor_speed * speed_factor;  // 应用指数加速

        }
    }
}





void STAR_MOTOR(void)
{
    HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, GPIO_PIN_SET);
}
void CLOSE_MOTOR(void)
{
    HAL_GPIO_WritePin(MOTOR_1_GPIO_Port, MOTOR_1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MOTOR_2_GPIO_Port, MOTOR_2_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MOTOR_3_GPIO_Port, MOTOR_3_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MOTOR_4_GPIO_Port, MOTOR_4_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENA_GPIO_Port, ENA_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(ENB_GPIO_Port, ENB_Pin, GPIO_PIN_RESET);
}